/*
 * LI_Calib: An Open Platform for LiDAR-IMU Calibration
 * Copyright (C) 2020 Jiajun Lv
 * Copyright (C) 2020 Kewei Hu
 * Copyright (C) 2020 Jinhong Xu
 * Copyright (C) 2020 LI_Calib Contributors
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
#ifndef TRAJECTORY_MANAGER_H
#define TRAJECTORY_MANAGER_H

#include <kontiki/sensors/constant_bias_imu.h>
#include <kontiki/sensors/vlp16_lidar.h>
#include <kontiki/trajectory_estimator.h>
#include <kontiki/trajectories/split_trajectory.h>
#include <kontiki/trajectories/uniform_r3_spline_trajectory.h>
#include <kontiki/trajectories/uniform_so3_spline_trajectory.h>
#include <kontiki/measurements/gyroscope_measurement.h>
#include <kontiki/measurements/accelerometer_measurement.h>
#include <kontiki/measurements/lidar_surfel_point.h>
#include <kontiki/measurements/orientation_measurement.h>
#include <kontiki/measurements/position_measurement.h>
#include <kontiki/measurements/velocity_measurement.h>
#include <kontiki/measurements/lidar_edge_measurement.h>
#include <kontiki/measurements/lidar_plane_measurement.h>
#include <kontiki/measurements/lidar_pivotPointPlane_measurement.h>
#include <kontiki/measurements/imu_accbias_measurement.h>
#include <kontiki/measurements/imu_grybias_measurement.h>
#include <kontiki/measurements/relative_velocity_measurement.h>
#include "imu_processor/calibration.hpp"
#include <glog/logging.h>

#include <Eigen/Core>
#include <fstream>
#include <memory>
#include <vector>

namespace lio {

struct imuPoseAndVel {
  Eigen::Vector3d pos;
  Eigen::Quaterniond rot;
  Eigen::Vector3d vel;
  double time;
};

class TrajectoryManager {
  using IMUSensor = kontiki::sensors::ConstantBiasImu;
  using LiDARSensor = kontiki::sensors::VLP16LiDAR;

  using SO3TrajEstimator   = kontiki::TrajectoryEstimator<kontiki::trajectories::UniformSO3SplineTrajectory>;
  using R3TrajEstimator    = kontiki::TrajectoryEstimator<kontiki::trajectories::UniformR3SplineTrajectory>;
  using SplitTrajEstimator = kontiki::TrajectoryEstimator<kontiki::trajectories::SplitTrajectory>;

  using EdgeMeasurement         = kontiki::measurements::LiDAREdgeMeasurement<LiDARSensor>;
  using PlaneMeasurement        = kontiki::measurements::LiDARPlaneMeasurement<LiDARSensor>;
  using SurfMeasurement         = kontiki::measurements::LiDARSurfelPoint<LiDARSensor>;
  using PivotPointPlaneMeasurement = kontiki::measurements::LiDARPivotPointPlaneMeasurement<LiDARSensor>;
  
  using GyroMeasurement    = kontiki::measurements::GyroscopeMeasurement<IMUSensor>;
  using AccelMeasurement   = kontiki::measurements::AccelerometerMeasurement<IMUSensor>;

  using OrientationMeasurement  = kontiki::measurements::OrientationMeasurement;
  using PositionMeasurement     = kontiki::measurements::PositionMeasurement;
  using VelocityMeasurement     = kontiki::measurements::VelocityMeasurement;
  using AcceBiasMeasurement     = kontiki::measurements::IMUAcceBiasMeasurement;
  using GrysBiasMeasurement     = kontiki::measurements::IMUGrysBiasMeasurement;
  using RelativeVelocityMeasurement = kontiki::measurements::RelativeVelocityMeasurement<IMUSensor>;

public:
  // segAttribute 用来表示 static 和 active segments
  struct segAttribute {
    double t0;  // 该段样条曲线的起始时刻
    double dt;  // 样条节点间隔
    int n; 	   // dt的个数，也就是小的时间段的个数
    int fid;    // 该段样条曲线第一个控制点的id，这个id与t0对应的id是一样的
    segAttribute() : t0(-1.0), dt(-1.0), n(-1), fid(-1) {} 
  };

  segAttribute staticSeg, activeSeg, newestSeg;

public:
  typedef std::shared_ptr<TrajectoryManager> Ptr;
  using Result = std::unique_ptr<kontiki::trajectories::TrajectoryEvaluation<double>>;

  explicit TrajectoryManager(double start_time, double end_time,
                             double knot_distance = 0.02)
          : lidar_(std::make_shared<LiDARSensor>()),
            imu_(std::make_shared<IMUSensor>()),
            calib_param_manager(std::make_shared<CalibParamManager>()) {
    assert(knot_distance > 0 && "knot_distance should be lager than 0");

    double traj_start_time = start_time;
    double traj_end_time = end_time;
    traj_ = std::make_shared<kontiki::trajectories::SplitTrajectory>
            (knot_distance, knot_distance, traj_start_time, traj_start_time);
    initialTrajTo(traj_end_time);
  }

  void initialTrajTo(double max_time);
  void extendTrajTo(double ts, double te);
  void updateSegments(double ts, double te, int pre_knot);

  // 初始化新来的控制点
  void initNewControlPoint(std::vector<imuPoseAndVel> &pqv, std::vector<Eigen::Vector3d> &grys, std::vector<Eigen::Vector3d> &accs);
  // 整个样条系统初始化
  void splineSystemInit(std::vector<imuPoseAndVel> &pqv, std::vector<Eigen::Vector3d> &grys, std::vector<Eigen::Vector3d> &accs);
  // 将初始化后的T^I0_Im的控制点转换到重力系T^G_Im
  void initControlPointToGlobal();

  // 设置控制点初值
  void initContralPoint(double t, Eigen::Vector3d &p, Eigen::Quaterniond &q);
  void initFirstCP(Eigen::Vector3d &p, Eigen::Quaterniond &q);
  void initEndCP(Eigen::Vector3d &p, Eigen::Quaterniond &q);

  // 删除所给时间点之前的控制点，剩余的控制点要能够计算出所给时间点处的位姿，既删除的是[t0, t_untill)，剩余的是[t_untill, t_max]
  bool deleteContralPointUntill(double t_untill);

  // 初始化估计器
  void trajInit();
  void trajInitForNewContralPoint();

  // 添加一个 点线 测量值
  void addEdgeMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector3d &last_point_a_,
					                Eigen::Vector3d &last_point_b_, const double frame_time, double distance_weight);

  // 添加一个 点面 测量值
  void addPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector3d &plane_unit_norm_,
						               double d_, const double frame_time);

  // 添加 局部地图点面 测量值
  // void addPivotPointPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector4d &coeff_,
	// 					               double pivot_time_, const double frame_time);

  void addPivotPointPlaneMeasurement(Eigen::Vector3d &curr_point_, Eigen::Vector4d &coeff_,
						               Eigen::Vector3d &pivot_pos_, Eigen::Quaterniond &pivot_rot_, const double frame_time);
  
  // 添加 角速度 测量值
  void addGyroscopeMeasurements(std::vector<Eigen::Vector3d> &grys, std::vector<double> &times);

  // 添加 加速度 测量值
  void addAccelerometerMeasurement(std::vector<Eigen::Vector3d> &accls, std::vector<double> &times);

   // 添加 线速度 约束
  void addLinearVelocityMeasurement(std::vector<Eigen::Vector3d> &accls, std::vector<double> &times);

  // imu偏置的约束
  void addAcceAndGrysBiasMeasurement(Eigen::Vector3d &ba, Eigen::Vector3d &bg, double weight_a, double weight_g);

  // 添加初始位置的四元数约束
  void addInitQuaternionMeasurement();

  // 所有测量值添加完毕，进行优化
  void solveOptProblem(bool &convergence);

  // 将优化后的外参保存
  void SetCalibParam();

  void ShowCalibParam();

  // 根据控制点以及时间给出 lidar 在 I_0 系下的位姿
  bool evaluateLidarPose(double lidar_time, Eigen::Vector3d& p, Eigen::Quaterniond& q) const;

  // 计算两个时刻 lidar 的相对位姿，T_2^1, p^1 = T_2^1 * p^2
  bool evaluateLidarRelativePose(double lidar_time1, double lidar_time2,
                                Eigen::Quaterniond &q_L2toL1, Eigen::Vector3d &p_L2inL1) const;

  // 根据重力方向的 roll 和 pitch 计算 lidar 在水平坐标系中的位姿
  bool evaluateLidarPoseAlignGravity(double lidar_time, Eigen::Vector3d& p, Eigen::Quaterniond& q) const;                  

  // 计算 I_0 系到水平世界系的旋转
  Eigen::Quaterniond evaluateRotateAlignGravity() const;

  // 计算 IMU 在水平世界系下的速度
  bool evaluateImuVelocityAlignGravity(double t, Eigen::Vector3d &v) const;

  // 计算 IMU 在水平世界系下的位姿
  bool evaluateImuPoseAlignGravity(double t, Eigen::Vector3d& p, Eigen::Quaterniond& q) const;

  // 计算 IMU 在I0系下的位姿
  bool evaluateImuPose(double t, Eigen::Vector3d& p, Eigen::Quaterniond& q) const;

  // 计算 IMU 在I0系下的速度
  bool evaluateImuVelocity(double t, Eigen::Vector3d &v) const;

  // 计算 IMU 在Im系下的线加速度
  bool eavluateImuAcceleration(double t, Eigen::Vector3d &a) const;

  // 计算 IMU 在Im系下的角速度
  bool eavluateImuGryscope(double t, Eigen::Vector3d &w) const;
  
  // 获取外参 T_L^I
  bool getExtrinsic(Eigen::Vector3d& p, Eigen::Quaterniond& q) const;
  
  /// Access the trajectory
  std::shared_ptr<kontiki::trajectories::SplitTrajectory> getTrajectory() const {
    return traj_;
  }

  double evaluateResidual();
  double evaluateResidual(std::vector<ceres::internal::ResidualBlock *> &res);
  void clearResidual();
  std::vector<ceres::internal::ResidualBlock *>& getResidualIds();

  void printErrorStatistics(const std::string& intro, bool show_gyro,
                      bool show_accel, bool show_lidar) const;

  
  CalibParamManager::Ptr calib_param_manager;

private:

  std::shared_ptr<kontiki::trajectories::SplitTrajectory> traj_;
  std::shared_ptr<kontiki::sensors::VLP16LiDAR> lidar_;
  std::shared_ptr<kontiki::sensors::ConstantBiasImu> imu_;

  std::shared_ptr<SplitTrajEstimator> estimator_split;  // 用于优化
  std::vector< std::shared_ptr<PlaneMeasurement>>  plane_list_; // 保存测量值，需要保存智能指针
  std::vector< std::shared_ptr<EdgeMeasurement>>  edge_list_;
  std::vector< std::shared_ptr<PivotPointPlaneMeasurement>>  pivotPlane_list_;
  std::vector< std::shared_ptr<GyroMeasurement>>  gyro_list_;
  std::vector< std::shared_ptr<AccelMeasurement>> accel_list_;
  std::vector< std::shared_ptr<OrientationMeasurement>> orien_list_;
  std::vector< std::shared_ptr<PositionMeasurement>> position_list_;
  std::vector< std::shared_ptr<VelocityMeasurement>> vel_list_;
  std::vector< std::shared_ptr<AcceBiasMeasurement>> ba_list_;
  std::vector< std::shared_ptr<GrysBiasMeasurement>> bg_list_;
  std::vector< std::shared_ptr<RelativeVelocityMeasurement>> relative_vel_list_;
  std::shared_ptr<OrientationMeasurement> m_q0_;  // 初始化t0处的四元数测量,为单位四元数(1 0 0 0);
  std::shared_ptr<PositionMeasurement> m_p0_; // 初始化t0处的位置测量，为（0 0 0）
};
}

#endif
